#ifndef IMU_PREINTEGRATION_H
#define IMU_PREINTEGRATION_H
#include <iostream>
#include <vector>

#include "state.h"
#include "se2.h"

struct Options {
   Options() {}
   double init_bg_ = 0.0;  // 初始零偏
   Eigen::Vector2d init_ba_ = Eigen::Vector2d::Zero();  // 初始零偏
   double noise_gyro_ = 1e-5;       // 陀螺噪声，标准差
   double noise_acce_ = 0.1;       // 加计噪声，标准差
};

class ImuPreIntegration {
public:
   ImuPreIntegration(Options options = Options());
   void Integrate(const Eigen::Vector3d& last_imu, const Eigen::Vector3d& curr_imu, const double& dt);

   State Predict(const State& last_state);

   SO2 GetDeltaRotation(const double &bg);
   Eigen::Vector2d GetDeltaVelocity(const double &bg, const Eigen::Vector2d &ba);
   Eigen::Vector2d GetDeltaPosition(const double &bg, const Eigen::Vector2d &ba);
   void Clear(const Options& options);

public:
    double dt_ = 0; 
    Eigen::Matrix<double, 5, 5> cov_ = Eigen::Matrix<double, 5, 5>::Zero();
    Eigen::Matrix3d noise_gyro_acce_ = Eigen::Matrix3d::Zero();

    Eigen::Vector2d ba_ = Eigen::Vector2d::Zero();
    double bg_;

    SO2 dR_;
    Eigen::Vector2d dv_ = Eigen::Vector2d::Zero();
    Eigen::Vector2d dp_ = Eigen::Vector2d::Zero();  

    double dR_dbg_ = 0;
    Eigen::Vector2d dV_dbg_ = Eigen::Vector2d::Zero();
    Eigen::Matrix2d dV_dba_ = Eigen::Matrix2d::Zero();
    Eigen::Vector2d dP_dbg_ = Eigen::Vector2d::Zero();
    Eigen::Matrix2d dP_dba_ = Eigen::Matrix2d::Zero();               
};

#endif 
